#include <iostream>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <aruco/cameraparameters.h>
#include <aruco/dictionary.h>
#include "debug.h"
#include "markermapper.h"
#include "sglviewer.h"
#if WIN32
#include <win_dirent.h>
#else
#include <dirent.h>
#endif
using namespace std;

class CmdLineParser
{
    int argc;
    char **argv;

public:
    CmdLineParser(int _argc, char **_argv) : argc(_argc), argv(_argv) {}
    bool operator[](string param)
    {
        int idx = -1;
        for (int i = 0; i < argc && idx == -1; i++)
            if (string(argv[i]) == param)
                idx = i;
        return (idx != -1);
    }
    string operator()(string param, string defvalue = "-1")
    {
        int idx = -1;
        for (int i = 0; i < argc && idx == -1; i++)
            if (string(argv[i]) == param)
                idx = i;
        if (idx == -1)
            return defvalue;
        else
            return (argv[idx + 1]);
    }
};

vector<string> readDir(string path)
{
    DIR *dir;
    struct dirent *ent;
    vector<string> res;
    if ((dir = opendir(path.c_str())) != NULL)
    {
        /* print all the files and directories within directory */
        while ((ent = readdir(dir)) != NULL)
            res.push_back(path + string("/") + string(ent->d_name));
        closedir(dir);
    }
    // check
    return res;
}
void readfiles(vector<string> &rgbs, string path)
{
    fstream fs(path + "/file.txt", std::ios::in);

    while (1)
    {
        string n, n2;
        fs >> n >> n2;
        if (!fs.good())
            break;
        rgbs.push_back(n);
        rgbs.push_back(n2);
    }
    // rgbs.pop_back
}
int main(int argc, char **argv)
{
    CmdLineParser cml(argc, argv);
    if (argc < 6 || cml["-h"])
    {
        cerr << "Usage:  directoryWithImages camera_parameters.yml   marker_size(meters)  dictionary out  [-ref id]  [-c arucoConfig.yml]  " << endl;
        cerr << "\tDictionaries: ";
        for (auto dict : aruco::Dictionary::getDicTypes())
            cerr << dict << " ";
        cerr << endl;
        return -1;
    }
    aruco_mm::debug::Debug::setLevel(3);
    aruco::CameraParameters Camera;
    Camera.readFromXMLFile(argv[2]);
    float markerSize = atof(argv[3]);
    string dict = argv[4];
    string outBaseName = argv[1];

    int ref_Marker_Id = -1;
    if (argc >= 7)
        if (string(argv[6]) == "-ref")
        {
            ref_Marker_Id = stoi(argv[7]);
        }
    // aruco_mm::debug::Debug::setLevel( stoi(cml("-debug","10")));
    // start processing
    auto AMM = aruco_mm::MarkerMapper::create();
    AMM->setParams(Camera, markerSize, ref_Marker_Id);
    // if (cml["-c"])
    //     AMM->getMarkerDetector().loadParamsFromFile(cml("-c"));
    // else
    cout << dict << endl;
    AMM->getMarkerDetector().setDictionary(dict);
    //    AMM->getMarkerDetector().setDetectionMode(aruco::DM_FAST,0.02);
    char key = 0;
    cv::Mat image, image2;
    //        std::rotate(files.begin(),files.begin()+10,files.end());
    // rotate vector
    int frameidx = 0;

    vector<string> rgbs;
    readfiles(rgbs, outBaseName);
    for (auto &t : rgbs)
    {
        cout << t << endl;
    }
    int cntts = rgbs.size() / 2;
    int wait = atoi(argv[8]);
    for (size_t i = 0; i < cntts && key != 27; i++)
    {
        string files = outBaseName + rgbs[2 * i + 1]; // cv::format("%s/rgb/%s.png", argv[1], i);
        cerr << "Reading..." << files << endl;
        image = cv::imread(files);
        if (image.empty())
            break;
        if (image.rows != Camera.CamSize.height || image.cols != Camera.CamSize.width)
        {
            cerr << "wont process THIS image because is not of the dimensions of the camera calibration file provided" << endl;
            continue;
        }
        AMM->process(image, frameidx++);
        AMM->drawDetectedMarkers(image, 3);
        // cv::resize(image, image2, cv::Size(1280, 960));
        cv::imshow("image", image);
        key = cv::waitKey(wait);
    }

    // finish processing

    AMM->optimize();
    //        AMM->saveToFile(outBaseName+".amm");
    // AMM->getFrameSet().saveToFile("fset.fset");
    AMM->saveToPcd(outBaseName + "/cloud.pcd", true);
    AMM->saveFrameSetPosesToFile(outBaseName + "/pose.yml");
    AMM->getCameraParams().saveToFile(outBaseName + "/cam.yml");
    AMM->getMarkerMap().saveToFile(outBaseName + "/maker.yml");

    // OpenCvMapperViewer Viewer;
    // aruco::MarkerMap mmap;
    // mmap.readFromFile(outBaseName + ".yml");
    // Viewer.setParams(mmap, 1.5, 1280, 960, "map_viewer");
    // key = 0;
    // while (key != 27)
    //     key = Viewer.show();
    printf("over\n");
}
